#pragma once
#include "ros/ros.h"
#include "nav_msgs/Path.h"
#include "nav_msgs/Odometry.h"
#include "align_enu/align_enu.h"
#include "std_srvs/Trigger.h"
#include "cloud_pose_msg/CloudPose.h"

class align_ENU_Wrapper
{
private:
    
  
    ros::Subscriber lidar_odom_sub;
    ros::Subscriber gnss_imu_odom_sub;
    ros::Subscriber init_q_sub;
    ros::Publisher cloud_enu_pub;
    align_ENU ae;
    // ros::ServiceServer optimization_srv;
    // ros::ServiceServer save_path_srv;
    
    std::vector<Eigen::Vector3d>lidar_origin_path;
    nav_msgs::Path gnss_path;
    Eigen::Quaterniond q;
    Eigen::Vector3d t;
    
    ros::Publisher cloud_pub;
    ros::Publisher lidar_path_pub;
    ros::Publisher gnss_path_pub;
    ros::Publisher trans_by_enu_path;
    ros::Publisher trans_by_imu_path;
    //status
    bool converge = false;
    std::vector<Eigen::Vector3d> gnss;
    bool init_q_vailed = false;
    Eigen::Quaterniond init_imu_q;
    //param
    std::string lidar_odom_topic;
    std::string gnss_imu_odom_topic;
    bool visualization;
    float opt_distance = 20.0;
    int opt_frame_nums = 40;
    int opt_times = -1;
    std::vector<double> extrinT;
    std::vector<double> extrinR;
    Eigen::Quaterniond extrin_q;
    Eigen::Vector3d extrin_t;
    
    
public:
    align_ENU_Wrapper(/* args */);
    ~align_ENU_Wrapper();
    void lidar_odom_sub_cb(const cloud_pose_msg::CloudPoseConstPtr&msg);
    void gnss_imu_odom_sub_cb(const nav_msgs::Odometry::ConstPtr&msg);
    void run();
    void init_imu_q_sub_cb(const geometry_msgs::Quaternion&msg);
    // bool optimization_srv_cb(std_srvs::Trigger::Request&req,std_srvs::TriggerResponse&res);
    void pub_visualization_msg(const cloud_pose_msg::CloudPose &msg);
    // bool save_path_cb(std_srvs::Trigger::Request&r,std_srvs::Trigger::Response &re);
};
